#ifndef GYRO_FLOW_PORT_ROLLING_SHUTTER_CORRECT_H
#define GYRO_FLOW_PORT_ROLLING_SHUTTER_CORRECT_H

#include <stdint.h>
#include "utils/BasicType.h"
#include "utils/keyframe.h"
#include "rotate_compute.h"
#include "QueueUtil.h"

typedef struct SNode {
    int32_t time_stamp;
    Quaternion q;
    Quaternion diff_q;
} SmoothNode;

typedef struct INode {
    int32_t time_stamp;
    int32_t img_addr;
} ImgNode;

void test_rolling_shutter_correct();

float compute_interpolate_weight(float x, Easing easing);

Quaternion org_quat_at_timestamp(Queue* imu_queue, uint32_t imu_length, int32_t time_stamp);

Matrix3 get_new_k(Matrix3* camera_matrix, float fov = 1.f);

void quats_at_timestamp(Queue* imu_queue, uint32_t imu_length, uint32_t img_timestamp,
                   Quaternion* q_imgsmooth_img, Quaternion* q_img_w, Quaternion* q_w_row);

Quaternion smoothed_quat_at_timestamp(Queue* imu_queue, uint32_t imu_length, int32_t time_stamp);

//Matrix3 compute_correct_rolling_shutter_matrix(SmoothNode* imu_queue,
//                                               ImgNode* img,
//                                               Matrix3* intrinsics,
//                                               Matrix3* extrinsics_cam_imu,
//                                               Matrix3* extrinsics_imu_cam,
//                                               int32_t row_time_stamp);

Matrix3 q_compute_correct_rolling_shutter_matrix(Queue* imu_queue,
                                                 int img_timestamp,
                                                 Matrix3* intrinsics,
                                                 Matrix3* intrinsics_inv,
                                                 Matrix3* extrinsics_cam_imu,
                                                 Matrix3* extrinsics_imu_cam,
                                                 int32_t row_time_stamp);

#endif //GYRO_FLOW_PORT_ROLLING_SHUTTER_CORRECT_H
